
class Integrator(object):

    WEIGHT = 0.3
    MAX_DELTA_V = 1000000    
    
    def __init__(self, nodeNum, forceUtil, parameters):
        self.forceUtil = forceUtil
        self.vel = forceUtil.genList(nodeNum)
        #self.timestep = timestep
        #self.MAX_DELTA_V = max_vel
        self.p = parameters

    def setParameter(self, p):
        self.p = p

    def calVV(self):
        y = self.forceUtil
        acc = 0.0
        for v in self.vel:
            acc += y.norm(v)
        return acc/len(self.vel)

class EulerIntegrator(Integrator):
    def __init__(self, nodeNum, forceUtil, parameters):
        #super(Integrator, self).__init__(self, nodeNum, forceUtil, timestep)
        Integrator.__init__(self, nodeNum, forceUtil, parameters)
        
    def integrate(self, loc, forces, nodefs):
        return self.force_location_eula(loc, self.vel, forces, nodefs, self.p.TimeStep, self.forceUtil)
    
    #@classmethod
    def force_location_eula(self, loc, vel, forces, nodefs, timestep, ForceUtil):
        y = ForceUtil
        for idx, f in enumerate(forces):
            #if idx==0: continue
            nf = nodefs[idx]
            #vel[idx] = y.add(y.mul(vel[idx], self.WEIGHT),
            #                 y.mul(f, timestep/nf.m*(1-self.WEIGHT)))
            #vel[idx] = y.add(y.mul(vel[idx], 0.6), y.mul(f, timestep/nf.m)) # best solution
            vel[idx] = y.mul(f, timestep/nf.m)
            #vel[idx] = y.ceilR(vel[idx], self.p.MAX_VEL)
            #vel[idx] = y.add(vel[idx], y.ceilR(y.mul(f, timestep/nf.m), self.MAX_DELTA_V))
            #vel[idx] = y.add(vel[idx], y.mul(f, timestep/nf.m))
            # 20140405 Solution
            #vel[idx] = y.ceilR(y.add(vel[idx], y.mul(f, timestep/nf.m)), self.MAX_DELTA_V)
            # bug!
            # loc[idx] = y.add(loc[idx], vel[idx]*timestep)
            loc[idx] = y.add(loc[idx], y.mul(vel[idx], timestep))
        return loc

class RungeKuttaIntegrator(Integrator):
    def __init__(self, nodeNum, forceUtil, forceCalculator, timestep, max_vel):
        Integrator.__init__(self, nodeNum, forceUtil, timestep, max_vel)
        self.cal = forceCalculator
        
        self.testLoc = forceUtil.genList(nodeNum)
        self.k0 = forceUtil.genList(nodeNum)
        self.k1 = forceUtil.genList(nodeNum)
        self.k2 = forceUtil.genList(nodeNum)
        self.k3 = forceUtil.genList(nodeNum)
        self.l0 = forceUtil.genList(nodeNum)
        self.l1 = forceUtil.genList(nodeNum)
        self.l2 = forceUtil.genList(nodeNum)
        self.l3 = forceUtil.genList(nodeNum)
        
        
    def integrate(self, loc, forces, nodefs):
        y = self.forceUtil
        t = self.timestep
        coef = [t/nf.m for nf in nodefs]
        f0 = forces
        
        for i, v in enumerate(self.vel):
            self.k0[i] = y.mul(v, t)
            self.l0[i] = y.mul(f0[i], coef[i])
            self.testLoc[i] = y.add(loc[i], y.mul(self.k0[i], 0.5))
        f1 = self.cal(self.testLoc)

        for i in xrange(len(self.vel)):
            v_tmp = self.calVel(y, self.vel[i], y.mul(self.l0[i], 0.5))
            self.k1[i] = y.mul(v_tmp, t)
            self.l1[i] = y.mul(f1[i], coef[i])
            self.testLoc[i] = y.add(loc[i], y.mul(self.k1[i], 0.5))
        f2 = self.cal(self.testLoc)

        for i in xrange(len(self.vel)):
            v_tmp = self.calVel(y, self.vel[i], y.mul(self.l1[i], 0.5))
            self.k2[i] = y.mul(v_tmp, t)
            self.l2[i] = y.mul(f2[i], coef[i])
            self.testLoc[i] = y.add(loc[i], y.mul(self.k2[i], 0.5))
        f3 = self.cal(self.testLoc)

        for i in xrange(len(self.vel)):
            v_tmp = self.calVel(y, self.vel[i], self.l2[i])
            self.k3[i] = y.mul(v_tmp, t)
            self.l3[i] = y.mul(f3[i], coef[i])

            tmp = self.calRunge(y, self.k0[i], self.k1[i], self.k2[i], self.k3[i])
            loc[i] = y.add(loc[i], tmp)
            v_tmp = self.calRunge(y, self.l0[i], self.l1[i], self.l2[i], self.l3[i])
            self.vel[i] = self.calVel(y, y.mul(self.vel[i], 1), v_tmp)
        return loc

    @staticmethod
    def calRunge(y, k0, k1, k2, k3):
        tmp0 = y.mul(y.add(k0, k3), 1.0/6.0)
        tmp1 = y.mul(y.add(k1, k2), 1.0/3.0)
        return y.add(tmp0, tmp1)

    #@classmethod
    def calVel(self, y, vel, deltav): 
        #vel = y.mul(vel, 0.6)
        #deltav = y.ceilR(deltav, self.MAX_DELTA_V)
        # Origin
        #return y.add(vel, deltav)
        #return y.ceilR(y.add(vel, deltav), self.MAX_DELTA_V)
        return deltav
        #return y.ceilR(y.add(vel, deltav), 1.0)
